import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node, PushRosNamespace
from launch.actions import (
    IncludeLaunchDescription,
    GroupAction,
    DeclareLaunchArgument,
    TimerAction,
    LogInfo,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition


def generate_launch_description():

    use_livox = LaunchConfiguration("use_livox")
    rviz_config_file = os.path.join(
        get_package_share_directory("bringup"), "rviz", "rviz2.rviz"
    )

    # 声明配置选项
    declare_use_livox = DeclareLaunchArgument(
        "use_livox", default_value="false", description="Whether to launch Livox MID360"
    )

    bringup_cmd_group = GroupAction(
        [
            # Livox-MID360
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(
                        get_package_share_directory("livox_ros_driver2"),
                        "launch_ROS2",
                        "msg_MID360_launch.py",
                    )
                ),
                condition=IfCondition(use_livox),
            ),
            # lio-sam
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(
                        get_package_share_directory("lio_sam"),
                        "launch",
                        "run.launch.py",
                    )
                )
            ),
            # nav2
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    os.path.join(
                        get_package_share_directory("navigation"),
                        "launch",
                        "nav2_launch.py",
                    )
                )
            ),
        ]
    )

    mid360_node = Node(
        package="bringup",
        executable="livox_mid360",
        name="livox_mid360",
        output="screen",
    )

    map_build_2d = Node(
        package="lidar_2d",
        executable="map_builder",
        name="map_builder",
        output="screen",
    )

    rviz = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        arguments=["-d", rviz_config_file],
        output="screen",
    )

    ld = LaunchDescription()
    ld.add_action(mid360_node)
    ld.add_action(map_build_2d)
    ld.add_action(declare_use_livox)
    ld.add_action(bringup_cmd_group)
    ld.add_action(rviz)

    return ld
